/**************************************************************************************************
* Title: SumBoundingVolumes.cpp
* Author: Gael Huber
* Description: Encapsulating header for bounding volumes and aggregate functions for intersection
*	tests.
**************************************************************************************************/
#include "SumBoundingVolumes.h"

/**************************************************************************************************
* AABB vs. AABB
**************************************************************************************************/
bool AABBAABBCollision(const AxisAlignedBox& a, const AxisAlignedBox& b) {
	// Test overlaps on all axe
	if(abs(a.position().x - b.position().x) > a.dimensions().x + b.position().x) return false;
	if(abs(a.position().y - b.position().y) > a.dimensions().y + b.position().y) return false;
	if(abs(a.position().z - b.position().z) > a.dimensions().z + b.position().z) return false;
	return true;
}

/**************************************************************************************************
* AABB vs. OBB
**************************************************************************************************/
bool AABBOBBCollision(const AxisAlignedBox& a, const ObjectAlignedBox& b) {
	// Convert AABB to OBB
	ObjectAlignedBox obb = ObjectAlignedBox(a.position(), a.dimensions(), Vector3_16f(1.0f, 0.0f, 0.0f), 
		Vector3_16f(0.0f, 1.0f, 0.0f), Vector3_16f(0.0f, 0.0f, 1.0f));
	return OBBOBBCollision(obb, b);
}

/**************************************************************************************************
* AABB vs. Bounding Sphere
**************************************************************************************************/
bool AABBSphereCollision(const AxisAlignedBox& a, const BoundingSphere& b) {
	if(abs(a.position().x - b.position().x) > a.dimensions().x + b.radius()) return false;
	if(abs(a.position().y - b.position().y) > a.dimensions().y + b.radius()) return false;
	if(abs(a.position().z - b.position().z) > a.dimensions().z + b.radius()) return false;
	return true;
}

// AABB vs. Bounding Cylinder
//bool AABBCylinderCollision(const AxisAlignedBox& a, const BoundingCylinder& b);

/**************************************************************************************************
* OBB vs. OBB
**************************************************************************************************/
bool OBBOBBCollision(const ObjectAlignedBox& a, const ObjectAlignedBox& b) {
	float ra, rb;
	Matrix_16f R, AbsR;

	// Compute rotation matrix expressing b in a's coordinate frame
	for(int i = 0; i < 3; ++i) {
		for(int j = 0; j < 3; ++j) {
			R.m[i][j] = Vec3Dot_16f(&a.orientation()[i], &b.orientation()[j]);
		}
	}

	// Compute translation vector t
	Vector3_16f t;
	Vec3Sub_16f(&t, &b.position(), &a.position());

	// Bring translation into a's coordinate frame
	t = Vector3_16f(Vec3Dot_16f(&t, &a.orientation()[0]), Vec3Dot_16f(&t, &a.orientation()[1]), Vec3Dot_16f(&t, &a.orientation()[2]));

	// Compute common subexpressions. Add in an epsilon term to counteract arithmetic errors when two
	// edges are parallel and their cross product is (near) null
	for(int i = 0; i < 3; ++i) {
		for(int j = 0; j < 3; ++j) {
			AbsR.m[i][j] = abs(R.m[i][j] + FLT_EPSILON);
		}
	}

	// Test axes L = A0, L = A1, L = A2
	for(int i = 0; i < 3; ++i) {
		ra = a.dimensions()[i];
		rb = b.dimensions()[0]*AbsR.m[i][0] + b.dimensions()[1]*AbsR.m[i][1] + b.dimensions()[2]*AbsR.m[i][2];
		if(abs(t[i]) > ra + rb) {
			return false;
		}
	}

	// Test axes L = B0, L = B1, L = B2
	for(int i = 0; i < 3; ++i) {
		ra = a.dimensions()[0]*AbsR.m[0][i] + a.dimensions()[1]*AbsR.m[1][i] + a.dimensions()[2]*AbsR.m[2][i];
		rb = b.dimensions()[i];
		if(abs(t[0]*R.m[0][i] + t[1]*R.m[1][i] + t[2]*R.m[2][i]) > ra + rb) {
			return false;
		}
	}

	// Test axis L = A0 x B0
	ra = a.dimensions()[1]*AbsR.m[2][0] + a.dimensions()[2]*AbsR.m[1][0];
	rb = b.dimensions()[1]*AbsR.m[0][2] + b.dimensions()[2]*AbsR.m[0][1];
	if(abs(t[2]*R.m[1][0] - t[1]*R.m[2][0]) > ra + rb) {
		return false;
	}

	// Test axis L = A0 x B1
	ra = a.dimensions()[1]*AbsR.m[2][1] + a.dimensions()[2]*AbsR.m[1][1];
	rb = b.dimensions()[0]*AbsR.m[0][2] + b.dimensions()[2]*AbsR.m[0][0];
	if(abs(t[2]*R.m[1][1] - t[1]*R.m[2][1]) > ra + rb) {
		return false;
	}

	// Test axis L = A0 x B2
	ra = a.dimensions()[1]*AbsR.m[2][2] + a.dimensions()[2]*AbsR.m[1][2];
	rb = b.dimensions()[0]*AbsR.m[0][1] + b.dimensions()[1]*AbsR.m[0][0];
	if(abs(t[2]*R.m[1][2] - t[1]*R.m[2][2]) > ra + rb) {
		return false;
	}

	// Test axis L = A1 x B0
	ra = a.dimensions()[0]*AbsR.m[2][0] + a.dimensions()[2]*AbsR.m[0][0];
	rb = b.dimensions()[1]*AbsR.m[1][2] + b.dimensions()[2]*AbsR.m[1][1];
	if(abs(t[0]*R.m[2][0] - t[2]*R.m[0][0]) > ra + rb) {
		return false;
	}

	// Test axis L = A1 x B1
	ra = a.dimensions()[0]*AbsR.m[2][1] + a.dimensions()[2]*AbsR.m[0][1];
	rb = b.dimensions()[0]*AbsR.m[1][2] + b.dimensions()[2]*AbsR.m[1][0];
	if(abs(t[0]*R.m[2][1] - t[2]*R.m[0][1]) > ra + rb) {
		return false;
	}

	// Test axis L = A1 x B2
	ra = a.dimensions()[0]*AbsR.m[2][2] + a.dimensions()[2]*AbsR.m[0][2];
	rb = b.dimensions()[0]*AbsR.m[1][1] + b.dimensions()[1]*AbsR.m[1][0];
	if(abs(t[0]*R.m[2][2] - t[2]*R.m[0][2]) > ra + rb) {
		return false;
	}

	// Test axis L = A2 x B0
	ra = a.dimensions()[0]*AbsR.m[1][0] + a.dimensions()[1]*AbsR.m[0][0];
	rb = b.dimensions()[1]*AbsR.m[2][2] + b.dimensions()[2]*AbsR.m[2][1];
	if(abs(t[1]*R.m[0][0] - t[0]*R.m[1][0]) > ra + rb) {
		return false;
	}

	// Test axis L = A2 x B1
	ra = a.dimensions()[0]*AbsR.m[1][1] + a.dimensions()[1]*AbsR.m[0][1];
	rb = b.dimensions()[0]*AbsR.m[2][2] + b.dimensions()[2]*AbsR.m[2][0];
	if(abs(t[1]*R.m[0][1] - t[0]*R.m[1][1]) > ra + rb) {
		return false;
	}

	// Test axis L = A2 x B2
	ra = a.dimensions()[0]*AbsR.m[1][2] + a.dimensions()[1]*AbsR.m[0][2];
	rb = b.dimensions()[0]*AbsR.m[2][1] + b.dimensions()[1]*AbsR.m[2][0];
	if(abs(t[1]*R.m[0][2] - t[0]*R.m[1][2]) > ra + rb) {
		return false;
	}

	return true;
}

// OBB vs. Bounding Sphere
//bool OBBSphereCollision(const ObjectAlignedBox& a, const BoundingSphere& b);

// OBB vs. Bounding Cylinder
//bool OBBCylinderCollision(const ObjectAlignedBox& a, const BoundingCylinder& b);

/**************************************************************************************************
* Bounding Sphere vs. Bounding Sphere
**************************************************************************************************/
bool SphereSphereCollision(const BoundingSphere& a, const BoundingSphere& b) {
	Vector3_16f dist;
	float radii = a.radius() + b.radius();
	return Vec3LengthSqr_16f(Vec3Sub_16f(&dist, &a.position(), &b.position())) <= radii * radii;
}

// Bounding Sphere vs. Bounding Cylinder
//bool SphereCylinderCollision(const BoundingSphere& a, const BoundingCylinder& b);

/**************************************************************************************************
* Bounding Cylinder vs. Bounding Cylinder
**************************************************************************************************/
//bool CylinderCylinderCollision(const BoundingCylinder& a, const BoundingCylinder& b);
